clc;close all;

% cd('mec_sys_id\');

%% 更新工程配置参数
project_config_updata;

%% 配置参数读取
search_max_nx = 8; %辨识对象阶数 搜索最大阶数 固定阶数
T_unit = 0.001;  %离线波形采集的时间单位是ms

if pc.disp_figure  == 0
    figure('Visible', 'off');
end


%% 搜索匹配文件
% 指定要搜索的文件名前缀
prefix = 'mec_id_data';
extension = '.csv';
% 获取当前目录中的所有文件
files = dir();

% 初始化一个空的 cell 数组来存储符合条件的文件名
matchingFiles = {};

% 遍历文件列表并找出匹配的文件
for i = 1:length(files)
    fileName = files(i).name;
    % 检查文件名是否以指定前缀开头并且是 .csv 文件
    if startsWith(fileName, prefix) && endsWith(fileName, extension)
        matchingFiles{end + 1} = fileName;  
    end
end

% 显示匹配的文件
disp('匹配的文件:');
disp(matchingFiles);


%% 导入辨识数据
filename = matchingFiles{1}; % 替换为你的CSV文件名  

data = readmatrix(filename);
tr = data(:,1) * T_unit;  % 转化成s
ur = data(:,2);
yr = data(:,3);

% 去掉离线波形第一个点
tr = tr(2:end);
ur = ur(2:end);
yr = yr(2:end);

%% 对原始信号进行处理
% 使用速度信号
t = tr(3:end);  
u = ur(3:end) * pc.kt_NM_A;  %当前电流信号 A

pos_red = (yr - yr(1)) / pc.enc_line_p_n * 2*pi;  %转化为位置单位  rad

pos_red_sgo = sgolayfilt(pos_red, 3, 11);  %多项式拟合
dify = (pos_red_sgo(3:end) - pos_red_sgo(1:end-2)) / (2 * pc.ts_s); % 中心差分方法, 这将减少两个点的长度


% t = tr(2:end);  
% u = ur(2:end);  %当前电流信号 A
% dify = diff(pos_red);

y = dify;

subplot(2,2,1);
plot(t,u,t,y);
legend('input u','filt y','response y');
title('System input and output raw data');
grid on;

%% 辨识系统模型，辨识模型输出和实际模型输出对比
subplot(2,2,2);
% Create object for time series data
estimationData = iddata(y, u, pc.ts_s);

% 自动搜索拟合阶数，选择匹配度最大的模型 
 
% 设定传递函数模型的阶数 
num_poles = 2:2:search_max_nx;
% num_zeros = num_poles;
num_zeros = num_poles-2;

orders = [num_poles', num_zeros'];

models = cell(size(orders, 1), 1);
fit_percents = zeros(size(orders, 1), 1);

for i = 1:size(orders, 1)
    % 创建传递函数模型
    model = tfest(estimationData, orders(i, 1), orders(i, 2));
    models{i} = model;
    
    % 估算模型的拟合度
    fit_percents(i) = model.Report.Fit.FitPercent;
end

% 找到匹配度最高的传递函数模型

% % 使用 find 函数找到第一个大于 pc.search_end_fit_percent 的值的索引
best_index = find(fit_percents > pc.search_end_fit_percent , 1);

% 查找并显示第一个大于 pc.search_end_fit_percent 的值
if ~isempty(best_index)
    first_value = fit_percents(best_index);
    fprintf('第一个大于 pc.search_end_fit_percent  的值是: %d\n', first_value);
else
    disp('没有找到大于 pc.search_end_fit_percent  的值');
end

% % 使用最高匹配度模型
% [~, best_index] = max(fit_percents);

best_model = models{best_index};

% 显示匹配度最高的模型
disp('匹配度最高的传递函数模型为:');
disp(best_model);
disp(['拟合度: ', num2str(fit_percents(best_index)), '%']);

compare(estimationData, best_model);

%% 归一化分子
G_normalized = best_model;
display(G_normalized);


%% 计算系统带宽
bw = bandwidth(G_normalized);
disp(['Bandwidth: ', num2str(bw), ' Hz']);


%% 辨识模型转化为离散模型和原始模型对比伯德图
subplot(2,2,3);
% Convert model from continuous to discrete time
sys_d = c2d(G_normalized, pc.ts_s);

% Visualize the results
if pc.disp_figure  == 1
    bodeplot(G_normalized, sys_d);
    legend('Original Model','Converted Model');
    grid on;    
end

%% 阶跃响应 离散模型和辨识连续模型对比
subplot(2,2,4);
if pc.disp_figure  == 1
    step(G_normalized, sys_d);
    legend('identified model sys_id','discretized model sys_d');
    grid on;
end

%% 如果是二阶系统 计算系统惯量
if best_index == 1
    ZH = tf_div(G_normalized);
    % 对二阶传递函数分解
    ZH = tf_div(G_normalized);
    H1 = ZH(1);
    H2 = ZH(2);
    display(H1);
    display(H2);
    %选择时间常数更大的传递函数
    [num1, den1] = tfdata(H1, 'v');     % 获取原始分子和分母系数
    tt1 = den1(1) / den1(2);
    
    [num2, den2] = tfdata(H2, 'v');     % 获取原始分子和分母系数
    tt2 = den2(1) / den2(2);
    
    if tt1 < tt2
        disp('H2 is J B tf');
        HRL = H2;  %H2为 机械传递函数
    else
        disp('H1 is J B tf');
        HRL = H1;  %H1为 机械传递函数
    end

    [num_rl, den_rl] = tfdata(HRL, 'v');     % 获取原始分子和分母系数
    J = den_rl(1);
    B = den_rl(2);
    display(J);
    display(B);
end


% 定义日志文件名
if best_index == 1
    logFileName = 'mec_id_logfile.txt';
    logMessage(logFileName, ['J is ',num2str(J),' kg*m^2']);
    logMessage(logFileName, ['B is ',num2str(B),' N*m/(rad/s)']);
end

%% 返回工程路径
% cd('..\');

% % %% 将辨识的传递函数 导入到simulink 模型
% set_motor_tf;



